#include "geometry_msgs/msg/transform_stamped.hpp"
#include "memory"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "robot_interfaces/action/robot_collection.hpp"
#include "robot_interfaces/msg/robot_target.hpp"
#include "tf2/utils.h"
#include "tf2_ros/transform_broadcaster.h"
#include <cmath>
#include <tf2/convert.h>
#include <thread>
class RobotController : public rclcpp::Node {
  using GoalHandle_ = rclcpp_action::ServerGoalHandle<
      robot_interfaces::action::RobotCollection>;

public:
  RobotController(const std::string &node_name) : rclcpp::Node(node_name) {
    this->declare_parameter("micro_robot_speed", 0.03);
    float micro_speed;
    this->get_parameter("micro_robot_speed", micro_speed);
    this->v = v;
    RCLCPP_INFO(this->get_logger(), "i am robot controller node");
    using namespace std::placeholders;
    target_sub_ = this->create_subscription<robot_interfaces::msg::RobotTarget>(
        "robot_target", 10,
        std::bind(&RobotController::target_callback, this,
                  std::placeholders::_1));
    this->action_server_ =
        rclcpp_action::create_server<robot_interfaces::action::RobotCollection>(
            this, "robot_collection",
            std::bind(&RobotController::handle_goal, this, _1, _2),
            std::bind(&RobotController::handle_cancel, this, _1),
            std::bind(&RobotController::handle_accepted, this, _1));
    this->tf_broadcaster_ =
        std::make_unique<tf2_ros::TransformBroadcaster>(*this);
  }

private:
  rclcpp_action::GoalResponse handle_goal(
      const rclcpp_action::GoalUUID &uuid,
      std::shared_ptr<const robot_interfaces::action::RobotCollection::Goal>
          goal) {
    RCLCPP_INFO(this->get_logger(), "Received goal request");
    (void)uuid;
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }
  rclcpp_action::CancelResponse
  handle_cancel(const std::shared_ptr<GoalHandle_> goal_handle) {
    RCLCPP_INFO(this->get_logger(), "Received cancel request");
    (void)goal_handle;
    return rclcpp_action::CancelResponse::ACCEPT;
  }
  void execute(const std::shared_ptr<GoalHandle_> goal_handle) {
    RCLCPP_INFO(this->get_logger(), "Executing goal");
    const auto goal = goal_handle->get_goal();
    auto result =
        std::make_shared<robot_interfaces::action::RobotCollection::Result>();
    geometry_msgs::msg::TransformStamped micro_robot;
    rclcpp::Rate rate(100);
    micro_robot_x = base_x;
    micro_robot_y = base_y;
    while (sqrt(pow(micro_robot_x - target_x, 2) +
                pow(micro_robot_y - target_y, 2)) > 0.5) {
      micro_robot.header.stamp = this->now();
      micro_robot.header.frame_id = "world";
      micro_robot.child_frame_id = "target";
      micro_robot.transform.translation.x = this->micro_robot_x;
      micro_robot.transform.translation.y = this->micro_robot_y;
      tf2::Quaternion quat_micro;
      quat_micro.setRPY(0.0, 0.0, 0.0);
      micro_robot.transform.rotation.x = quat_micro.x();
      micro_robot.transform.rotation.y = quat_micro.y();
      micro_robot.transform.rotation.w = quat_micro.w();
      tf_broadcaster_->sendTransform(micro_robot);
      float tan_theta_s =
          pow((micro_robot_y - target_y) / (micro_robot_x - target_x), 2);
      float p_n_x = (micro_robot_x - target_x) > 0 ? 1 : -1;
      float p_n_y = (micro_robot_y - target_y) > 0 ? 1 : -1;
      float cos_theta = std::sqrt(1.0 / (tan_theta_s + 1.0));
      float sin_theta = std::sqrt(tan_theta_s / (tan_theta_s + 1.0));
      this->micro_robot_x += p_n_x * this->v * cos_theta;
      this->micro_robot_y += p_n_y * this->v * sin_theta;
      auto feedback = std::make_shared<
          robot_interfaces::action::RobotCollection::Feedback>();
      feedback->distance_to_robot = sqrt(pow(micro_robot_x - target_x, 2) +
                                         pow(micro_robot_y - target_y, 2));
      goal_handle->publish_feedback(feedback);
      if (goal_handle->is_canceling()) {
        RCLCPP_INFO(this->get_logger(), "Goal is being cancelled");
        result->succeed_collect = false;
        goal_handle->canceled(result);
        return;
      }
      RCLCPP_INFO(this->get_logger(), "publishing feedback");
      rate.sleep();
    }
    result->succeed_collect = true;
    goal_handle->succeed(result);
    RCLCPP_INFO(this->get_logger(), "Goal succeeded");
  }
  void handle_accepted(const std::shared_ptr<GoalHandle_> goal_handle) {
    using std::placeholders::_1;
    std::thread{std::bind(&RobotController::execute, this, _1), goal_handle}
        .detach();
  }
  rclcpp::Subscription<robot_interfaces::msg::RobotTarget>::SharedPtr
      target_sub_;
  rclcpp_action::Server<robot_interfaces::action::RobotCollection>::SharedPtr
      action_server_;
  void
  target_callback(const robot_interfaces::msg::RobotTarget::SharedPtr msg) {
    target_x = msg->target_world_x;
    target_y = msg->target_world_y;
    base_x = msg->base_world_x;
    base_y = msg->base_world_y;
  }
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  float v = 0.03;
  float micro_robot_x = 0.0;
  float micro_robot_y = 0.0;
  float base_x = 0.0;
  float base_y = 0.0;
  float target_x = 0.0;
  float target_y = 0.0;
};
int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<RobotController>("robot_controller");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}